#!/usr/bin/env python

PACKAGE = 'quickmcl'

from math import pi, radians

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# Define level constants
LVL_MM = gen.const("LEVEL_MOTION_MODEL", int_t, 0x1, "Internal use")["value"]
LVL_SM = gen.const("LEVEL_SENSOR_MODEL", int_t, 0x2, "Internal use")["value"]
LVL_PF = gen.const("LEVEL_PARTICLE_FILTER", int_t, 0x4, "Internal use")["value"]
LVL_ROS = gen.const("LEVEL_ROS", int_t, 0x8, "Internal use")["value"]

resampling_enum = gen.enum([
    gen.const("low_variance", str_t, "low_variance", "Low variance sampling"),
    gen.const("adaptive", str_t, "adaptive", "Adaptive resampling"),
    gen.const("kld", str_t, "kld", "Adaptive KLD resampling")
], "Enum for resampling types")

mm = gen.add_group("motion_model")
#      Name                    Type      Lvl  Description                   Default   Min     Max
mm.add("motion_model_alpha_1", double_t, LVL_MM, "Rotation noise from rotation", 0.05, 0, None)
mm.add("motion_model_alpha_2", double_t, LVL_MM, "Rotation noise from translation", 0.1, 0, None)
mm.add("motion_model_alpha_3", double_t, LVL_MM, "Translation noise from translation", 0.02, 0, None)
mm.add("motion_model_alpha_4", double_t, LVL_MM, "Translation noise from rotation", 0.05, 0, None)
mm.add("motion_model_min_trans", double_t, LVL_MM, "Minimum translation for filter to update [m]", 0.2, 0, None)
mm.add("motion_model_min_rot", double_t, LVL_MM, "Minimum rotation for filter to update [rad]", pi / 6, 0, 2 * pi)

sm = gen.add_group("sensor_model")
sm.add("likelihood_z_hit", double_t, LVL_SM,
       "Probability weight to assign to 'hit when there is actually something there in the map'.",
       0.9, 0, 1)
sm.add("likelihood_z_rand", double_t, LVL_SM,
       "Probability weight to assign to 'random hit even when there is nothing there in the map'.",
       0.1, 0, 1)
# Can not be changed after initialisation (embedded in internal map)
# sm.add("likelihood_sigma_hit", double_t, LVL_SM,
#        "Sigma of hit distribution [m]",
#        0.1, 0, None)
# sm.add("likelihood_max_obstacle_distance", double_t, LVL_SM,
#        "Maximum distance to compute obstacle distances for (for likelihood map) [m].",
#        2.0, 0, None)
sm.add("likelihood_num_beams", int_t, LVL_SM,
       "Number of laser beams to use from the scan. This sub-sampling will be done via selecting evenly spaced beams."
       " Set to 0 to use all (not recommended).",
       30, 0, None)
sm.add("likelihood_max_laser_distance", double_t, LVL_SM,
       "Maximum laser distance [m]. Used to divide likelihood_z_rand to create proper probability distribution.",
       14.0, 0, None)

pf = gen.add_group("particle_filter")
pf.add("particle_filter_particle_count_min", int_t, LVL_PF, "Number of particles (min)", 100, 10, None)
pf.add("particle_filter_particle_count_max", int_t, LVL_PF, "Number of particles (max)", 5000, 10, None)
pf.add("particle_filter_resample_count", int_t, LVL_PF, "How often (in filter updates) to resample.", 2, 1, None)
pf.add("particle_filter_alpha_fast", double_t, LVL_PF, "Low pass constant for adaptive/kld.", 0.1, 0, 1)
pf.add("particle_filter_alpha_slow", double_t, LVL_PF, "Low pass constant for adaptive/kld.", 0.001, 0, 1)
pf.add("particle_filter_kld_epsilon", double_t, LVL_PF, "KLD epsilon parameter", 0.05, 0, 1)
pf.add("particle_filter_kld_z", double_t, LVL_PF, "KLD z_(1-delta) parameter", 0.95, 0, 1)
pf.add("space_partitioning_resolution_xy", double_t, LVL_PF, "KLD bucket resolution x and y [m]", 0.5, 0, 1)
pf.add("space_partitioning_resolution_theta", double_t, LVL_PF, "KLD bucket resolution rotation [rad]",
       radians(10.0), 0, 2 * pi)
pf.add("particle_filter_resample_type", str_t, LVL_PF, "Resample algorithm to use", "kld", edit_method=resampling_enum)

ros = gen.add_group("ros")
ros.add("fixed_frame", str_t, LVL_ROS, "TF frame to work in, map expected to be in this frame.", "map")
ros.add("localised_frame", str_t, LVL_ROS, "TF frame to localise.", "base_link")
ros.add("odom_frame", str_t, LVL_ROS, "TF frame to publish transform for (from fixed frame).", "odom")

ros.add("save_pose_period", double_t, LVL_ROS,
        "How often (in seconds) to save the pose to the parameter server. This will be reloaded on startup."
        " Set to 0 to disable saving.",
        0.0, 0, None)
ros.add("post_date_transform", double_t, LVL_ROS,
        "How much to post-date transform, should be similar to delta between scans."
        " Helps rviz get less glitchy and in general indicates that this estimate is valid a bit into the future.",
        0.1, 0, None)
ros.add("publish_particles", bool_t, LVL_ROS,
        "If true, publish filter particles as markers.",
        False)
# Can not be changed after initialisation (results in instantiating different classes)
# ros.add("internal_laser_processing", bool_t, LVL_ROS,
#         "If true, process laser directly from a scan internally instead of relying on external conversion to a"
#         " point cloud.",
#         False)

exit(gen.generate(PACKAGE, PACKAGE, "QuickMCL"))
